/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CAMERA_H
#define CAMERA_H

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include "common_include.h"

/**
 * @todo write docs
 */
namespace myslam{

class Camera
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<Camera> Ptr;
    
    double fx_=0, fy_=0, cx_=0, cy_=0, baseline_=0; //mono don't need baseline
    SE3 pose_;         //extrinsic of camera;
    SE3 pose_inv_;     //inverse of extrinsics;
    
    Camera();
    Camera(double fx, double fy, double cx, double cy , double baseline, const SE3 &pose):fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose){pose_inv_=pose_.inverse();}
    
    SE3 pose() const { return pose_;}
    
    //return intrinsic matrix
    Mat33 K() const{
        Mat33 k;
        k<<fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
        return k;
    }
    
    //coordinate transform: world, camera, pixel
    Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w);
    
    Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w);
    
    Vec2 camera2pixel(const Vec3 &p_c);
    
    Vec3 pixel2camera(const Vec2 &p_p, double depth=1);
    
    Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth=1);
    
    Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w);
    
};

}
#endif // CAMERA_H
